import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.io.OutputStream;

import lejos.util.Timer;
import lejos.util.TimerListener;

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.SensorPortListener;
import lejos.nxt.Sound;
//import lejos.nxt.SoundSensor;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;

public class ClientManager implements SensorPortListener
{
	public char delimiter = '\n';
	private NXTCommands comm;
	public DataOutputStream dos;
	public UltraSensorTimerListener sensorTimerTask;
	public boolean _connected = false;
	TouchSensor _bumper = new TouchSensor(SensorPort.S1);
	public ClientManager() {
		try {
			String connected = "Connected";
	        String waiting = "Waiting...";
	        String closing = "Closing...";
	        
			while (true)
			{
				LCD.drawString(waiting, 0, 0);
				LCD.refresh();

		        BTConnection btc = Bluetooth.waitForConnection(0, NXTConnection.PACKET);
		        _connected = true;
		        Sound.beep();
		        
				LCD.clear();
				LCD.drawString(connected, 0, 0);
				LCD.refresh();	

				DataInputStream dis = btc.openDataInputStream();
				dos = btc.openDataOutputStream();
				//btc.op
				
				comm = new NXTCommands(this);
				
				SensorPort.S1.addSensorPortListener(this);
				
				//UltraSensorThread ultraThread = new UltraSensorThread(this);
				//ultraThread.start();

				sensorTimerTask = new UltraSensorTimerListener(this);
				Timer timer = new Timer(20, sensorTimerTask);
				timer.start();
				//Sound.buzz();
				
				byte c;
				String message = "";
		
				while(dos != null && dis != null && (c = dis.readByte()) != -1)
				{
					if((char)c == delimiter){
						comm.receivedMessage(message);
						message = "";
					} else {
						message = message + String.valueOf((char)c);
						//LCD.drawString(message, 0, 1);
						//LCD.refresh();
					}
					
				}
				
				dis.close();
				dos.close();
				Thread.sleep(100); // wait for data to drain
				LCD.clear();
				LCD.drawString(closing, 0, 0);
				LCD.refresh();
				btc.close();
				LCD.clear();
				_connected = false;
				Sound.buzz();
				//break;
			}
		} catch (IOException e) {
			// Handle
		} catch (InterruptedException e) {
			// Handle
		} finally {}
	}
	
	public void sendMessage(String message){
		try {
			dos.writeBytes(message + "\n");
			dos.flush();
		} catch (IOException e) {
		}
	}
	
	public void stateChanged(SensorPort arg0, int arg1, int arg2) {
		if(arg0 == SensorPort.S1) {
			if(_bumper.isPressed()) {
				Sound.buzz();
				comm.stopMotors();
				sendMessage("BUMP");
			}
		}
		//else if(arg0 == SensorPort.S2) {
			/*_ultra.ping();
			Sound.pause(30);
			int dist = _ultra.getDistance();
			sendMessage("SCRS 0.0 " + String.valueOf(dist));
			*/
		//}
	}
	
	/*class UltraSensorThread extends Thread {
		ClientManager _clientManager;
		UltrasonicSensor _ultra = new UltrasonicSensor(SensorPort.S2);
		int rotSpeed = 150;
		int maxAngle = 60;
		public UltraSensorThread(ClientManager client) {
			_clientManager = client;
			Motor.C.setSpeed(rotSpeed);
			Motor.C.smoothAcceleration(true);
		}
		public void run(){
			
			while (_clientManager._connected) {
				if(!Motor.C.isMoving()) {
					if(Motor.C.getTachoCount() < 0) {
						Motor.C.rotateTo(maxAngle, true);
					} else {
						Motor.C.rotateTo(-maxAngle, true);
					}
				}
				int dist = _ultra.getDistance();
				int angle = Motor.C.getTachoCount();
				//sendMessage("SCRS " + String.valueOf(angle) + " " + String.valueOf(dist));
				Sound.pause(30);
			}
			
			Motor.C.rotateTo(0);
		}
	}
	*/
	class UltraSensorTimerListener implements TimerListener {
		ClientManager _clientManager;
		UltrasonicSensor _ultra = new UltrasonicSensor(SensorPort.S2);
		int rotSpeed = 150;
		int maxAngle = 90;
		private boolean _sensorOn = false;
		public UltraSensorTimerListener(ClientManager clientManager) {
			_clientManager = clientManager;
			Motor.C.setSpeed(rotSpeed);
			Motor.C.smoothAcceleration(true);
		}
		public void timedOut() {
			//Sound.buzz();
			if (_clientManager._connected && _sensorOn) {
				if(!Motor.C.isMoving()) {
					if(Motor.C.getTachoCount() < 0) {
						Motor.C.rotateTo(maxAngle, true);
					} else {
						Motor.C.rotateTo(-maxAngle, true);
					}
				}
				int dist = _ultra.getDistance();
				int angle = Motor.C.getTachoCount();
				sendMessage("SCRS " + String.valueOf(angle) + " " + String.valueOf(dist));
				//Sound.pause(30);
				
			}
		}
		public void turnSensorOn(boolean on){
			_sensorOn = on;
			if(!on) {
				Motor.C.stop();
			}
		}
		public void setSensorSpeed(int newSpeed) {
			rotSpeed = newSpeed;
			Motor.C.setSpeed(rotSpeed);
		}
	}
}
